;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Author: Francesco Petrogalli
;; Email:  francesco.petrogalli@dmi.unipg.it
;; 
;; Dipartimento di Matematica e Informatica
;; Universita' degli Studi di Perugia
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Classi
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defclass image ()
  ((num-rows :initarg :num-rows :reader image-num-rows)
   (num-cols :initarg :num-cols :reader image-num-cols)
   (image-pixels :initarg :image-pixels :accessor image-pixels))
  (:documentation "Classe delle immagini"))

(defclass ant ()
  ((path :initarg :path :accessor ant-path)
   (path-energy :type number :initform 0 :accessor ant-path-energy))
  (:documentation "Formica"))

(defclass shift ()
  ((i :initarg :i :reader shift-i)
   (j :initarg :j :reader shift-j))
  (:documentation "Coppia ordinata (coordinate del pixel)"))

(defclass quark (shift)
  ((value :initarg :value :accessor quark-value))
  (:documentation "Coppia ordinata con peso"))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Istanziazione delle classi
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defgeneric create-image-from-file (filename fileformat)
  (:documentation "Crea una immagine da un file pgm"))

(defmethod create-image-from-file (filename (fileformat (eql 'pgm)))
  (with-open-file (in filename)
    (with-standard-io-syntax
	(progn
	  (read-line in) ; P2
	  (read-line in) ; comment #
	  (let* ((n (read in))
		 (m (read in))
		 (pixels (make-array (list m n) :element-type 'number)))
	    (read-line in) ; greys
	    (loop for i from 0 below m do
		 (loop for j from 0 below n do
		      (setf (aref pixels i j) (read in))))
	    (make-instance 'image 
			   :num-rows m 
			   :num-cols n 
			   :image-pixels pixels))))))

(defun create-ant (m n)
  (make-instance 'ant :path (make-array (list m n))))

(defgeneric create-shift (i j)
  (:documentation "Istanziazione di uno shift"))

(defmethod create-shift ((i integer) (j integer))
  (make-instance 'shift :i i :j j))

(defgeneric create-quark (sh value)
  (:documentation "Instanzia un quark"))

(defmethod create-quark ((sh shift) (value number))
  (make-instance 'quark :i (shift-i sh) :j (shift-j sh) :value value))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Metodi
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; Classe delle Immagini

(defgeneric image-get-pixel (img i j)
  (:documentation "Restituisce il pixel (i,j) di img"))

(defmethod image-get-pixel ((img image) (i integer) (j integer))
  (aref (image-pixels img) i j))

(defgeneric locate-color (img color-value)
  (:documentation "Return a list of pixel coordinates that match a given color."))

(defmethod locate-color ((img image) (color-value integer))
  (let* ((m (- (image-num-rows img) 1))
	 (n (- (image-num-cols img) 1))
	 (data (loop for i from 0 to m
		  nconc (loop for j from 0 to n
			   collect (list i j)))))
    (remove-if-not #'(lambda (x) (<= (abs (- (image-get-pixel img (first x) (second x))
					     color-value)) 
				     30)) data)))

(defgeneric save-image (filename img format)
  (:documentation "salvataggio di una immagine su file"))

(defmethod save-image (filename (img image) (format (eql 'pgm)))
  (with-open-file (out filename :direction :output :if-exists :supersede)
    (format out
	    "P2~%#made with LISP and Emacs~%~A ~A~%~A~%~{~A~%~}" 
	    (image-num-cols img) (image-num-rows img) 255 
	    (loop for k from 0 below (array-total-size (image-pixels img))
		 collecting (row-major-aref (image-pixels img) k)))))


(defgeneric compute-initial-energy (img1 img2)
  (:documentation "Calcolo energia iniziale
tra le due immagini (fedelta' ai dati"))

(defmethod compute-initial-energy ((img1 image) (img2 image))
  (loop for k from 0 below (array-total-size (image-pixels img1))
       summing (square (- (row-major-aref (image-pixels img1) k) (row-major-aref (image-pixels img2) k)))))

;; Classe delle coppie ordinate (shift)

(defgeneric shift-sum (sh1 sh2)
  (:documentation "Somma componente per componente di due shift"))

(defmethod shift-sum ((sh1 shift) (sh2 shift))
  (create-shift (+ (shift-i sh1) (shift-i sh2)) 
		(+ (shift-j sh1) (shift-j sh2))))

(defgeneric shift-difference (sh1 sh2)
  (:documentation "Somma componente per componente di due shift"))

(defmethod shift-difference ((sh1 shift) (sh2 shift))
  (create-shift (- (shift-i sh1) (shift-i sh2)) 
		(- (shift-j sh1) (shift-j sh2))))

(defgeneric shift-list (sh)
  (:documentation "Ritorna la lista di due lementi (i,j) dello shift"))

(defmethod shift-list ((sh shift))
  (list (shift-i sh) (shift-j sh)))

(defgeneric shift-abs (sh)
  (:documentation "Riporta il generico shift nel primo quadrante con i > j"))

(defmethod shift-abs ((sh shift))
  (let ((i (abs (shift-i sh)))
	(j (abs (shift-j sh))))
    (create-shift (max i j) (min i j))))

;; Classe dei quark

(defgeneric quark-shift (q)
  (:documentation "Restituisce lo shift associato al quark"))

(defmethod quark-shift ((q quark))
  (create-shift (shift-i q) (shift-j q)))

;; classe delle formiche
(defgeneric compute-path-energy (anty img1 img2)
  (:documentation "Calcola l'energia della soluzione determinata dalla formica"))


(defmethod compute-path-energy ((anty ant) (img1 image) (img2 image))
  (let ((path (ant-path anty))
	(m (- (image-num-rows img1) 1))
	(n (- (image-num-cols img1) 1)))
    (setf (ant-path-energy anty) 0)
    (loop for i from 0 to m do
	 (loop for j from 0 to n do
	      (let ((sh (quark-shift (aref path i j))))
		(incf (ant-path-energy anty) (compute-energy-term i j sh img1 img2 anty)))))))
		

(defgeneric compute-energy-term (i j sh img1 img2 anty)
  (:documentation "calcola un addendo della funzione di energia"))
 
(defmethod compute-energy-term ((i (eql 0)) (j (eql 0)) (sh shift) (img1 image) (img2 image) (anty ant))
  (energy-fidelity img1 img2 (create-shift i j) sh))

(defmethod compute-energy-term ((i (eql 0)) (j integer) (sh shift) (img1 image) (img2 image) (anty ant))
  (let ((base-pixel (create-shift i j)))
    (+ (energy-fidelity img1 img2 base-pixel sh)
       (energy-left base-pixel sh anty))))

(defmethod compute-energy-term ((i integer) (j (eql 0)) (sh shift) (img1 image) (img2 image) (anty ant))
  (let ((base-pixel (create-shift i j)))
    (+ (energy-fidelity img1 img2 base-pixel sh)
       (energy-top base-pixel sh anty))))

(defmethod compute-energy-term ((i integer) (j integer) (sh shift) (img1 image) (img2 image) (anty ant))
  (let ((base-pixel (create-shift i j)))
    (+ (energy-fidelity img1 img2 base-pixel sh)
       (energy-top base-pixel sh anty)
       (energy-left base-pixel sh anty))))

(defun pheromone-energy-relation-function (energy)
  (/ 10 energy))

(defgeneric save-pheromone-trail (anty)
  (:documentation "Deposizione della traccia di pheromone associata
al persorso calcolato"))

(defmethod save-pheromone-trail ((anty ant))
  (loop for j from 0 below (array-total-size *pheromone*)
     as ph-trail = (row-major-aref *pheromone* j) 
     for k from 0 below (array-total-size (ant-path anty))
     as sh = (shift-list (row-major-aref (ant-path anty) k))
     when (gethash sh ph-trail)
     do (setf (gethash sh ph-trail) (+ (ant-path-energy anty) (nth-value 0 (gethash sh ph-trail))))
     else do (setf (gethash sh ph-trail)  (pheromone-energy-relation-function (ant-path-energy anty)))))




;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Posizione dei pixel sull'immagine
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defgeneric pixel-exists (position sh)
  (:documentation "Determina se esiste il pixel posizionato 
top/left/bottom/right rispetto ad sh"))

(defmethod pixel-exists ((position (eql 'top)) (sh shift))
  (> (shift-i sh) 0))

(defmethod pixel-exists ((position (eql 'left)) (sh shift))
  (> (shift-j sh) 0))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Specializzazione della generic function 'print-object'
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defmethod print-object ((obj image) stream)
  (format stream "Immagine ~Ax~A" (image-num-rows obj) (image-num-cols obj)))

(defmethod print-object ((anty ant) stream)
  (format stream "Path Energy: ~A" (ant-path-energy anty)))

(defmethod print-object ((obj shift) stream)
  (format stream "(~A ~A)" (shift-i obj) (shift-j obj)))

(defmethod print-object ((obj quark) stream)
  (format stream "~A -> ~A" (quark-shift obj) (quark-value obj)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Gestione dei pesi
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defvar *weights* (make-hash-table  :test #'equalp ))

(defvar *q* 1.3)
(defvar *qq* (* *q* *q*))

(defun g-function (sh-list)
  (let* ((i (first sh-list))
	 (j (second sh-list))
	 (tt (sqrt (+ (* i i) (* j j)))))
    (cond
      ((< tt *q*) (expt tt 2))
      (t (- (+ 2 *q* tt) *qq*)))))

(defun add-g-term (sh-list)
  (setf (gethash sh-list *weights*) (g-function sh-list)))

(defgeneric g-term (sh)
  (:documentation "Ritorna il peso di uno shift"))

(defmethod g-term ((sh shift))
  (let ((key (shift-list (shift-abs sh))))
    (or (gethash key *weights*)
	(add-g-term key))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Euristica e funzioni energia
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;
;;Euristica:	    y ^
;;		      |
;;		    B +--
;;		      |	 \----
;;		      |	      \----
;;		      |		   \-----
;;		      |			 \----
;;		      |			      \----
;;    ----------------+----------------------------\------------>
;;		      |                            A            x
;;		      |
;;		      |
;;		      |
;;		      |
;;		      |
;;		      |
;; y = -(B/A)x+B

(defparameter *A* 10)
(defparameter *B* 1)

(defun eta (x)
  (cond
    ((>= x *A*) 0)
    (T (+ *B* (* x (- (/ *B* *A*)))))))


;; Quadrato
(defun square (x)
  (expt x 2))

(defgeneric energy-fidelity (img1 img2 base-pixel sh)
  (:documentation "Termine di fedelta' ai dati"))

(defmethod energy-fidelity ((img1 image) (img2 image) (base-pixel shift) (sh shift))
  (let (;tmp = (i,j)+(u_ij,v_ij)
	(tmp (shift-sum base-pixel sh))) 
    (square (- (image-get-pixel img1 (shift-i base-pixel) (shift-j base-pixel)) 
	       (image-get-pixel img2 (shift-i tmp) (shift-j tmp))))))

(defgeneric energy-top (base-pixel sh anty)
  (:documentation "Peso della differenza di sh con il pixel superiore"))

(defmethod energy-top ((base-pixel shift) (sh shift) (anty ant))
  (let* ((top-pixel (create-shift (- (shift-i base-pixel) 1) (shift-j base-pixel)))
	 (top-choice (quark-shift (aref (ant-path anty) (shift-i top-pixel) (shift-j top-pixel))))
	 (sh-diff (shift-difference top-choice sh)))
    (g-term sh-diff)))

(defgeneric energy-left (base-pixel sh anty)
  (:documentation "Peso della differenza di sh con il pixel di sinistra"))

(defmethod energy-left ((base-pixel shift) (sh shift) (anty ant))
  (let* ((top-pixel (create-shift (shift-i base-pixel) (- (shift-j base-pixel) 1)))
	 (top-choice (quark-shift (aref (ant-path anty) (shift-i top-pixel) (shift-j top-pixel))))
	 (sh-diff (shift-difference top-choice sh)))
    (g-term sh-diff)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Inizializzazione del programma                                             ;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defparameter *img2* (create-image-from-file "test-shifted45.pgm" 'pgm))
(defparameter *img1* (create-image-from-file "test45.pgm" 'pgm))

(defvar *image-solution*)

(defun init-solution (img)
  (let ((m (image-num-rows img))
	(n (image-num-cols img)))
    (setf *image-solution*  
	  (make-instance 'image 
			 :num-rows m 
			 :num-cols n 
			 :image-pixels (make-array (list m n) 
						   :element-type  'number
						   :initial-contents (loop for i from 1 to m collecting 
									  (loop for j from 1 to n collecting 255)))))))

(defparameter *alpha* 1.1)
(defparameter *beta* 1)
(defparameter *default-pheromone* 1.1)
(defparameter *rho* 0.9)
(defparameter *soglia* 0.001)

(defvar *pheromone*)
(defvar *ants*)

(defgeneric initialize-pheromone (img)
  (:documentation "Inizializza l'array con la traccia di feromone."))

(defmethod initialize-pheromone ((img image))
  (setf *pheromone* 
	(make-array (list (image-num-rows img) (image-num-cols img))))
  (loop for i from 0 below (array-total-size *pheromone*) do 
       (setf (row-major-aref *pheromone* i) (make-hash-table :test #'equalp))))

(defun pheromone-cleaning ()
  (loop for i from 0 below (array-total-size *pheromone*) do
       (with-hash-table-iterator (my-iterator (row-major-aref *pheromone* i))
	 (loop
	      (multiple-value-bind (more? key value) (my-iterator)
		(unless more? (return))
		(when (<= value *soglia*) (remhash key (row-major-aref *pheromone* i))))))))

(defun pheromone-evaporation ()
  (loop for i from 0 below (array-total-size *pheromone*) do
       (with-hash-table-iterator (my-iterator (row-major-aref *pheromone* i))
	 (loop
	      (multiple-value-bind (more? key value) (my-iterator)
		(unless more? (return))
		(setf (gethash key (row-major-aref *pheromone* i)) (* value *rho*)))))))

(defun get-best-flow-for-pixel (ph-trail)
  "Deterina lo spostamento migliore tra quelli presenti nella hash table
massimizzando la quantita' di feromone"
  (let ((sh-list '(0 0))
	(pher 0))
    (loop for key being the hash-keys of ph-trail
	 using (hash-value value) 
	 when (> value pher) do 
	 (progn 
	   (setf sh-list key)
	   (setf pher value)))
       sh-list))

(defun get-best-solution-from-pheromone ()
  "Determina la migliore soluzione a partire dalle tracce di feromone"
  (loop for j from 0 below (array-total-size *pheromone*)
     collecting (get-best-flow-for-pixel (row-major-aref *pheromone* j))))




(defun save-U-V-matrix (img)
  "Scrive le matrici U e V"
  (with-open-file (out "U.txt" :direction :output :if-exists :supersede)
    (let ((solution (get-best-solution-from-pheromone))
	  (n (image-num-cols img)))
      (do ((count 1 (1+ count))
	   (item (mapcar #'first solution) (rest item)))
	  ((null item) (format out "~%"))
	(format out "~A~:[ ~;~%~]" (first item) (= 0 (mod count n))))))
  (with-open-file (out "V.txt" :direction :output :if-exists :supersede)
    (let ((solution (get-best-solution-from-pheromone))
	  (n (image-num-cols img)))
      (do ((count 1 (1+ count))
	   (item (mapcar #'second solution) (rest item)))
	  ((null item) (format out "~%"))
	(format out "~A~:[ ~;~%~]" (first item) (= 0 (mod count n)))))))

(defun get-best-array-solution-from-pheromone (img)
  "determina un array bidimensionale con la soluzione migliore"
  (let ((m (image-num-rows img))
	(n (image-num-cols img)))
    (make-array (list m n) :element-type 'number
		:initial-contents (loop for k from 0 below m collecting 
				       (loop for i from 0 below n collecting
					    (get-best-flow-for-pixel (aref *pheromone* k i ) ))))))
		

(row-major-aref (image-pixels img) k)))))

(defun save-u-v-matrix-from-array (img)
  "salvataggio della matrice della soluzione"
  (with-open-file (out "U1.txt" :direction :output :if-exists :supersede)
    (let ((soluzione  (get-best-array-solution-from-pheromone img))
	  (n (image-num-cols img)))
      (loop for k from 0 below (array-total-size soluzione) 
	 as sh = (row-major-aref soluzione k) do
	   (format out "~A~:[ ~;~%~]" (first sh) (= 0 (mod (+ k 1) n))))))
  (with-open-file (out "V1.txt" :direction :output :if-exists :supersede)
    (let ((soluzione  (get-best-array-solution-from-pheromone img))
	  (n (image-num-cols img)))
      (loop for k from 0 below (array-total-size soluzione) 
	 as sh = (row-major-aref soluzione k) do
	   (format out "~A~:[ ~;~%~]" (second sh) (= 0 (mod (+ k 1) n)))))))

(defun indexes-are-ok (i j m n)
  "calcola il valore di verita' di 0\leq i<n e 0\leq j <n"
  (and (>= j 0)
       (< j n)
       (>= i 0)
       (< i m)))

(defun build-image-from-solution (img)
  (init-solution img)
  (let ((soluzione (get-best-array-solution-from-pheromone img))
	(n (image-num-cols *image-solution*))
	(m (image-num-cols *image-solution*)))
    (loop for i from 0 below m do
	 (loop for j from 0 below n do 
	      (let* ((sh (aref soluzione i j))
		     (new-i (+ i (first sh)))
		     (new-j (+ j (second sh))))
		(format t "~A~%" (list new-i new-j)))))))
		
(defgeneric initialize-ants (img number-of-ants)
  (:documentation "Inizializza le formiche"))

(defmethod initialize-ants ((img image) (number-of-ants integer))
  (setf *ants* 
	(loop for i from 1 to number-of-ants 
	   collecting (create-ant (image-num-rows img) (image-num-cols img)))))


;;; test initializations

(initialize-pheromone *img1*)

(initialize-ants *img1* 10)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; User interface                                                             ;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun system-message (text)
  (let ((rule "-----------------------------------"))
    (format t "~A~%" rule)
    (format t "| ~A~%" text)
    (format t "~A~%" rule)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;                                                                            ;;
;;                   ************* ALGORITMO *************                    ;;
;;                                                                            ;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defvar *displacements* (loop for i from 0 below 10 nconc 
			     (loop for j from 0 to 10 collecting (list i j))))

(defgeneric list-of-possible-choices (img1 img2 i j)
  (:documentation "Elenco di possibili spostamenti per il
pixel (i,j)"))

(defmethod list-of-possible-choices ((img1 image) (img2 image) (i integer) (j integer))
  (let* ((pheromone (aref *pheromone* i j))
	 (quark-trail (loop for key being the hash-keys of pheromone 
			 using (hash-value value)
			 collecting (create-quark 
				     (create-shift (first key) (second key)) 
				     value)))
	 ;(nuovi-pixel (remove-if #'(lambda (x) (gethash x pheromone)) (locate-color img2 (image-get-pixel img1 i j))))
	 (nuovi-pixel (remove-if #'(lambda (x) (gethash x pheromone)) *displacements*))
	 (new-quarks (loop for pixel in nuovi-pixel 
			collecting (create-quark (create-shift (- (first pixel) i) 
							       (- (second pixel) j)) 
						 *default-pheromone*))))
    (append quark-trail new-quarks)))

(defun pick-random-from-list (lista)
  (let ((n (random (length lista))))
    (nth n lista)))

(defgeneric choose-pixel (anty img1 img2 i j)
  (:documentation "Scelta del pixel i j per la formica anty 
sulle immmagini img1 e img2"))

(defmethod choose-pixel ((anty ant) (img1 image) (img2 image) (i (eql 0)) (j (eql 0)))
  (let ((list-of-choices (list-of-possible-choices img1 img2 i j))
	(base-pixel (create-shift i j)))
    (loop for qqq in list-of-choices do
	 (let* ((ph (quark-value qqq))
		(sh (quark-shift qqq))
		(energia (energy-fidelity img1 img2 base-pixel sh))
		(euristica (eta energia)))
	   (setf (quark-value qqq) (* (expt ph *alpha*) (expt euristica *beta*)))))
    (let ((S (loop for qqq in list-of-choices summing (quark-value qqq))))
      (cond
	((= S 0) (pick-random-from-list list-of-choices))
	(t (let ((R (random S)))
	     (cond 
	       ((= 0 R) (pick-random-from-list list-of-choices))
	       (t (loop for www in list-of-choices
		     when (< (quark-value www) R)
		     do (setf R (- R (quark-value www)))
		     else return www)))))))))
;;prima riga con j>0 i=0
(defmethod choose-pixel ((anty ant) (img1 image) (img2 image) (i (eql 0)) (j integer))
  (let ((list-of-choices (list-of-possible-choices img1 img2 i j))
	(base-pixel (create-shift i j)))
    (loop for qqq in list-of-choices do
	 (let* ((ph (quark-value qqq))
		(sh (quark-shift qqq))
		(energia (+ (energy-fidelity img1 img2 base-pixel sh)
			    (energy-left base-pixel sh anty)))
		(euristica (eta energia)))
	   (setf (quark-value qqq) (* (expt ph *alpha*) (expt euristica *beta*)))))
    (let ((S (loop for qqq in list-of-choices summing (quark-value qqq))))
      (cond
	((= S 0) (pick-random-from-list list-of-choices))
	(t (let ((R (random S)))
	     (cond 
	       ((= 0 R) (pick-random-from-list list-of-choices))
	       (t (loop for www in list-of-choices
		     when (< (quark-value www) R)
		     do (setf R (- R (quark-value www)))
		     else return www)))))))))

;; prima colonna con j=0 i>0
(defmethod choose-pixel ((anty ant) (img1 image) (img2 image) (i integer) (j (eql 0)))
  (let ((list-of-choices (list-of-possible-choices img1 img2 i j))
	(base-pixel (create-shift i j)))
    (loop for qqq in list-of-choices do
	 (let* ((ph (quark-value qqq))
		(sh (quark-shift qqq))
		(energia (+ (energy-fidelity img1 img2 base-pixel sh)
			    (energy-top base-pixel sh anty)))
		(euristica (eta energia)))
	   (setf (quark-value qqq) (* (expt ph *alpha*) (expt euristica *beta*)))))
    (let ((S (loop for qqq in list-of-choices summing (quark-value qqq))))
      (cond
	((= S 0) (pick-random-from-list list-of-choices))
	(t (let ((R (random S)))
	     (cond 
	       ((= 0 R) (pick-random-from-list list-of-choices))
	       (t (loop for www in list-of-choices
		     when (< (quark-value www) R)
		     do (setf R (- R (quark-value www)))
		     else return www)))))))))

;;resto dell'immagine con i>0 j>0
(defmethod choose-pixel ((anty ant) (img1 image) (img2 image) (i integer) (j integer))
  (let ((list-of-choices (list-of-possible-choices img1 img2 i j))
	(base-pixel (create-shift i j)))
    (loop for qqq in list-of-choices do
	 (let* ((ph (quark-value qqq))
		(sh (quark-shift qqq))
		(energia (+ (energy-fidelity img1 img2 base-pixel sh)
			    (energy-top base-pixel sh anty)
			    (energy-left base-pixel sh anty)))
		(euristica (eta energia)))
	   (setf (quark-value qqq) (* (expt ph *alpha*) (expt euristica *beta*)))))
    (let ((S (loop for qqq in list-of-choices summing (quark-value qqq))))
      (cond
	((= S 0) (pick-random-from-list list-of-choices))
	(t (let ((R (random S)))
	     (cond 
	       ((= 0 R) (pick-random-from-list list-of-choices))
	       (t (loop for www in list-of-choices
		     when (< (quark-value www) R)
		     do (setf R (- R (quark-value www)))
		     else return www)))))))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Testing
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


(defun ACOtest ()
  (loop for anty in *ants* do
       (progn 
	 (loop for i from 0 to (- (image-num-rows *img1*) 1) do
	      (loop for j from 0 to (- (image-num-cols *img1*) 1) do
		   (progn
		     (setf (aref (ant-path anty) i j) (choose-pixel anty *img1* *img2* i j))
					;(format t "~A,~A choosen...~%" i j)
		     )))
	 (compute-path-energy anty *img1* *img2*)
	 (pheromone-evaporation)
	 (save-pheromone-trail anty)
					;(pheromone-cleaning))))
	 )))


	 
